(defun test (pred &rest forms)
  (print '-)
  (map nil #'print forms)
  (apply pred forms))

(defun ~= (x y)
  (< (abs (- x y)) 0.1))

(defun ~=* (form &rest vals)
  (reduce #'(lambda (x y)
	      (and x y))
	  (mapcar #'~= form vals)))

; tests

(defun test-dist ()
  (test #'= (dist '(1 5) '(10 2)) (sqrt 90)))

(defun test-process-telemetry ()
  (and (test #'equalp
	     (process-telemetry "I dx dy time-limit min-sensor max-sensor max-speed max-turn max-hard-turn ; ")
	     '(dx dy max-speed max-hard-turn))
       (test #'equalp
	     (process-telemetry "T 3450 aL -234.040 811.100 47.5 8.450 b -220.000 750.000 12.000 m -240.000 812.000 90.0 9.100 ; ")
	     '(#\L -234.04 811.1 47.5 8.45 ((#\b -220.0 750.0 12.0)) ((#\m -240.0 812.0 90.0 9.1))))
       (test #'equalp
	     (process-telemetry "T time-stamp cL vehicle-x vehicle-y vehicle-dir vehicle-speed ; ")
	     '(#\L vehicle-x vehicle-y nil nil))
       (test #'null (process-telemetry "E t s ;"))))

(defun test-calc-dir ()
  (test #'~=
	(calc-dir '(0 0) '(4 4))
	(rad 45)))

(defun test-calc-dir-w-obstacles ()
  (and (test #'~=*
	     (multiple-value-list (calc-dir-w-obstacles '(10 10) '(0 . 0) 1))
	     14.1421 (rad 225))
       (let ((*map* '((#\b 9 8 1))))
	 (test #'~=*
	       (multiple-value-list (calc-dir-w-obstacles '(10 10) '(0 . 0) 1))
	       2.06 (rad 223)))
       (test #'~=* 
	     (multiple-value-list (calc-dir-w-obstacles '(10 10) (cons (rad 270) (rad 90)) 1))
	     14.1421 (rad 225))
       (test #'~=*
	     (multiple-value-list (calc-dir-w-obstacles '(-10 0) '(0 . 0) 1))
	     10 (rad 360))
       (test #'~=*
	     (multiple-value-list (calc-dir-w-obstacles '(10 10) (cons (rad 90) (rad 270)) 1))
	     14.1421 (rad 270))))

(defun test-in-range ()
  (and (test #'= (in-range pi (cons 3pi/2 pi/2)) 0)
       (test #'= (in-range pi (cons 3pi/2 pi/2)) 0)
       (test #'> (in-range (+ pi/2 0.01) (cons pi/2 3pi/2)) 0)
       (test #'= (in-range pi (cons pi/2 3pi/2)) 1)
       (test #'= (in-range pi '(0 0)) 0)))

(defun test-calc-escape-dir ()
  (let ((*x* 10)
	(*y* 10))
    (and (test #'~= (calc-escape-dir '(#\m 9 9 10)) (rad 135))
	 (test #'~= (calc-escape-dir '(#\m 9 9 5)) (rad 90))
	 (test #'~= (calc-escape-dir '(#\m 9 9 1)) (rad 35))
	 (test #'~= (calc-escape-dir '(#\m 9 9 0)) (rad 225)))))

(defun test-calc-closest-ms ()
  (let ((*x* 10) (*y* 10))
    (test #'equalp (calc-closest-ms '((#\m 9 9 0) 
				      (#\m 9 8 0)
				      (#\m 11 10 0)))
	  '((#\m 11 10 0) (#\m 9 9 0)))))